#pragma config(Hubs,  S1, HTMotor,  HTMotor,  none,     none)
#pragma config(Hubs,  S2, HTServo,  HTMotor,  HTMotor,  none)
#pragma config(Sensor, S1,     ,               sensorI2CMuxController)
#pragma config(Sensor, S2,     ,               sensorI2CMuxController)
#pragma config(Sensor, S3,     tLimits,        sensorI2CCustom9V)
#pragma config(Sensor, S4,     tIRBeacon,      sensorHiTechnicIRSeeker1200)
#pragma config(Motor,  motorA,          mTow,          tmotorNXT, PIDControl, encoder)
#pragma config(Motor,  motorB,          mLeftTow,      tmotorNXT, PIDControl, encoder)
#pragma config(Motor,  motorC,          mRightTow,     tmotorNXT, PIDControl, reversed, encoder)
#pragma config(Motor,  mtr_S1_C1_1,     dLeft1,        tmotorTetrix, openLoop, encoder)
#pragma config(Motor,  mtr_S1_C1_2,     dLeft2,        tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S1_C2_1,     dRight1,       tmotorTetrix, openLoop, reversed, encoder)
#pragma config(Motor,  mtr_S1_C2_2,     dRight2,       tmotorTetrix, openLoop, reversed)
#pragma config(Motor,  mtr_S2_C2_1,     aBallTip,      tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S2_C2_2,     aUExtend,      tmotorTetrix, openLoop, reversed)
#pragma config(Motor,  mtr_S2_C3_1,     aBallPickup,   tmotorTetrix, openLoop, reversed)
#pragma config(Motor,  mtr_S2_C3_2,     aLExtend,      tmotorTetrix, openLoop, reversed)
#pragma config(Servo,  srvo_S2_C1_1,    sBallDrop,            tServoStandard)
#pragma config(Servo,  srvo_S2_C1_2,    sTowGoal,             tServoStandard)
#pragma config(Servo,  srvo_S2_C1_3,    sIRSeeker,            tServoStandard)
#pragma config(Servo,  srvo_S2_C1_4,    servo4,               tServoNone)
#pragma config(Servo,  srvo_S2_C1_5,    servo5,               tServoNone)
#pragma config(Servo,  srvo_S2_C1_6,    servo6,               tServoNone)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

#include "JoystickDriver.c"  //Include file to "handle" the Bluetooth messages.
#include "FTC_Toolkit.h"

void initializeRobot()
{
  motor[dLeft1] = motor[dLeft2] = motor[dRight1] = motor[dRight2] = 0;
  motor[aBallTip] = motor[aUExtend] = motor[aBallPickup] = motor[aLExtend] = 0;
  servo[sBallDrop] = 255;
  servo[sIRSeeker] = 100;
  ImplementVariables();

  return;
}

task main()
{
  initializeRobot();
  waitForStart();

  //ReadEncoder();

  StartTask(ReleaseBoom);
  waitForLimit(lLOWEXTEND_LOW, false);
  wait1Msec(250);

  // Start and align to bottom of ramp
  EncoderDrive(25, 0, 8000, true);
  EncoderDrive(0, -60, DEG_90_TURN, true);
  waitForTask(&tReleaseBoom_Active);

  DriveSystem(-100, 0);
  wait1Msec(1000);
  DriveSystem(0, 0);
  wait1Msec(500);

  wait1Msec(1000);
  EncoderDrive( 45, 0, 850, true);
  wait1Msec(20);

  // Turn and move to first goal
  EncoderDrive(0, -60, DEG_90_TURN, true);
  StartTask(DropTow);
  EncoderDrive(-25, 0, 1000, true);
  waitForTask(&tDropTow_Active);
  EncoderDrive(-25, 0, 2500, false);
  wait1Msec(100);
  EncoderDrive(25, 0, 750, true);
  wait1Msec(100);

  // Score Ball
  StartTask(ScoreBall);
  waitForTask(&tScoreBall_Active);
  StartTask(HighExtendDown);

  // Move Goal out of the way
  EncoderDrive(0, -60, DEG_90_TURN, true);
  StartTask(RaiseTow);
  waitForTask(&tRaiseTow_Active);
  StartTask(HighExtendDown);
  EncoderDrive(-30, 0, 500, true);
  EncoderDrive(30, 0, 500, true);
  EncoderDrive(0, 60, DEG_180_TURN, true);
  wait1Msec(100);

  // Re-Align to wall
  waitForTask(&tHighExtendDown_Active);
  DriveSystem(-100, 0);
  wait1Msec(1000);
  DriveSystem(0, 0);
  wait1Msec(500);

  // Turn and move to first goal
  wait1Msec(1000);
  EncoderDrive( 30, 0, 600, true);
  wait1Msec(20);
  EncoderDrive(0, -60, DEG_90_TURN, true);
  StartTask(DropTow);
  EncoderDrive(-25, 0, 1000, true);
  waitForTask(&tDropTow_Active);
  StartTask(ReloadBall);
  EncoderDrive(-25, 0, 2500, false);
  wait1Msec(100);
  EncoderDrive(25, 0, 3000, true);
  wait1Msec(100);

  // Score Ball
  waitForTask(&tReloadBall_Active);
  StartTask(ScoreBall);
  waitForTask(&tScoreBall_Active);
  StartTask(HighExtendDown);


  // Move to Parking Zone
  EncoderDrive( 25, 0, 2000, true,);
  EncoderDrive( 0, 60, DEG_45_TURN, true);
  /*
  EncoderDrive( 25, 0, 5000, true);
  */

  while (true){
  	abortTimeslice();
  }

}
